/*********************************************************************
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2008, 2013, Willow Garage, Inc.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *
 * Author: Eitan Marder-Eppstein
 *         David V. Lu!!
 *********************************************************************/
#include <costmap_2d/obstacle_layer.h>
#include <costmap_2d/costmap_math.h>
#include <tf2_ros/message_filter.h>

#include <pluginlib/class_list_macros.h>
#include <sensor_msgs/point_cloud2_iterator.h>

// 障碍层插件和名称空间声明:
PLUGINLIB_EXPORT_CLASS(costmap_2d::ObstacleLayer, costmap_2d::Layer)

using costmap_2d::NO_INFORMATION;
using costmap_2d::LETHAL_OBSTACLE;
using costmap_2d::FREE_SPACE;

using costmap_2d::ObservationBuffer;
using costmap_2d::Observation;

namespace costmap_2d{

// 障碍层地图通过订阅传感器话题，将传感器输出的障碍物信息存进buffer（剔除过高、过远的点），在本层地图上将观测到的
// 点云标记为障碍物，将传感器到点云的连线上的点标记为FREE_SPACE。最后在bound范围内，将本层地图合并到主地图上

/*  通用代价地图中的障碍层常常是这样设置的:
obstacles:
 enabled: true
 obstacle_range: 2.0
 raytrace_range: 5.0
 track_unknown_space: true
 combination_method: 1
 observation_sources: laser_scan_sensor pointcloud2_sensor
 laser_scan_sensor:
    data_type: LaserScan
    topic: /scan
    marking: true
    clearing: true
    max_obstacle_height: 1.0
    min_obstacle_height: -1.0

 pointcloud2_sensor:
    topic:          /people_cloud
    data_type:      PointCloud2
    sensor_frame:   "/base_link"
    obstacle_range: 5.0           对能否及早发现障碍物至关重要 ，跟车速也有关系。 如果车速0.6，obstacle_range设置为1.5，连正常避障都实现不了，至少也得3
    raytrace_range: 5.0           增大raytrace_range， obstacle_range会增大数据量，影响代价地图刷新
    observation_persistence: 5.0
    marking:        true
    clearing:       true
    min_obstacle_height: 0.0      min_obstacle_height: 传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。决定是否将地面的部分加入到代价地图，默认0
    max_obstacle_height: 2.0
https://charon-cheung.github.io/2020/11/25/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/%E9%9A%9C%E7%A2%8D%E5%B1%821%20%20%E9%85%8D%E7%BD%AE%E5%92%8C%E5%B8%B8%E8%A7%81%E9%97%AE%E9%A2%98/#%E9%85%8D%E7%BD%AE%E5%8F%82%E6%95%B0
*/

/**
 * 代价地图中每个cell可用255个不同值中任何一个值，可是下层数据结构仅需要3个值。具体来说，每个cell仅需要3个值来表示cell的3种状态：free，occupied，unknown。 
 * 当投影到代价地图时候，每种状态被赋一个特定的代价值，也就是说 每个cell的cost值是由这个cell对应的各层中对应的cell的状态进行加权得到的。 如果列有一定量的占用
 * 就被赋代价值。如果存储的障碍物信息是3D的，需要将每一列的障碍物信息投影成2D后才能放入到代价地图。
 * 
*/
void ObstacleLayer::onInitialize(){
  // 创建多个节点句柄nh("~/" + name_)、g_nh、prefix_nh和source_node(nh, source)，
  // 通过方法param(param_name, param_val, default_val)从参数服务器检索指示的值param_name,
  // 若成功，则将结果存储在param_val中，否则将default_val赋值给param_val。
  ros::NodeHandle nh("~/" + name_), g_nh;
  rolling_window_ = layered_costmap_->isRolling();

  bool track_unknown_space;
  nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
  if (track_unknown_space)
    default_value_ = NO_INFORMATION;
  else
    default_value_ = FREE_SPACE;

  // 设置障碍物层的代价值 x值 y值 分辨率和原点坐标
  ObstacleLayer::matchSize();
  current_ = true;

  global_frame_ = layered_costmap_->getGlobalFrameID();
  double transform_tolerance;
  nh.param("transform_tolerance", transform_tolerance, 0.2);

  std::string topics_string;
  // 从参数服务器获取所订阅的话题
  nh.param("observation_sources", topics_string, std::string(""));
  ROS_INFO("    Subscribed to Topics: %s", topics_string.c_str());
  
  std::stringstream ss(topics_string);  // 使用字符串流,方便用空格分开话题

  std::string source;
  while (ss >> source)  {
    ros::NodeHandle source_node(nh, source);    // 指定一个父句柄和追加的命名空间，把 source 放入到 <node_namespace>/nh/source 命名空间

    // 从特定话题获取参数
    double observation_keep_time, expected_update_rate, min_obstacle_height, max_obstacle_height;
    std::string topic, sensor_frame, data_type;
    bool inf_is_valid, clearing, marking;

    source_node.param("topic", topic, source);
    source_node.param("sensor_frame", sensor_frame, std::string(""));
    // observation_persistence：一帧激光在队列中存在的时间，observation_persistence=0代表只用最新一帧激光
    source_node.param("observation_persistence", observation_keep_time, 0.0);
    // expected_update_rate：代表两帧激光之间间隔的最短时间
    source_node.param("expected_update_rate", expected_update_rate, 0.0);
    source_node.param("data_type", data_type, std::string("PointCloud"));
    // 传感器读数的最小高度（以米为单位）视为有效。通常设置为地面高度。决定是否将地面的部分加入到代价地图，默认0
    source_node.param("min_obstacle_height", min_obstacle_height, 0.0);
    // max_obstacle_height: 默认2，单位米。插入代价地图的障碍物的最大高度，应当比机器人高度稍微高一点，应该是用于有机械臂的情况。
    // 设置为大于全局max_obstacle_height参数的值将会失效，设置为小于全局max_obstacle_height的值将过滤掉传感器上大于该高度以的点。
    source_node.param("max_obstacle_height", max_obstacle_height, 2.0);
    //inf_is_valid: 默认是false，但是对点云障碍无效，仅对雷达scan有效:
    source_node.param("inf_is_valid", inf_is_valid, false);
    //“marking”和“clearing”参数确定传感器是否向代价地图添加障碍物信息或从代价地图中清除障碍物信息，或者同时都做
    source_node.param("clearing", clearing, false);
    source_node.param("marking", marking, true);

    if (!(data_type == "PointCloud2" || data_type == "PointCloud" || data_type == "LaserScan")){
      ROS_FATAL("Only topics that use point clouds or laser scans are currently supported");
      throw std::runtime_error("Only topics that use point clouds or laser scans are currently supported");
    }

    std::string raytrace_range_param_name, obstacle_range_param_name;

    // 确定传感器所能探测到障碍物的最大范围值
    // 设置机器人检测障碍物的最大范围，意思是说超过该范围的障碍物，并不进行检测，只有靠近到该范围内才把该障碍物当作影响路径规划和移动的障碍物。
    // 对能否及早发现障碍物至关重要 ，跟车速也有关系。 如果车速0.6，obstacle_range设置为1.5，连正常避障都实现不了，至少也得3
    double obstacle_range = 2.5;
    if (source_node.searchParam("obstacle_range", obstacle_range_param_name)){
      source_node.getParam(obstacle_range_param_name, obstacle_range);
    }

    // 确定传感器所能感知到周围环境的最大范围值
    // 在机器人移动过程中，实时清除代价地图上的障碍物的最大范围，更新可自由移动的空间数据。假如设置该值为3米，那么就意味着在3米内的障碍物，
    // 本来开始时是有的，但是本次检测却没有了，那么就需要在代价地图上来更新，将旧障碍物的空间标记为可以自由移动的空间
    // 增大raytrace_range， obstacle_range会增大数据量，影响代价地图刷新
    double raytrace_range = 3.0;
    if (source_node.searchParam("raytrace_range", raytrace_range_param_name)){
      source_node.getParam(raytrace_range_param_name, raytrace_range);
    }

    ROS_DEBUG("Creating an observation buffer for source %s, topic %s, frame %s", source.c_str(), topic.c_str(), sensor_frame.c_str());

    /*******************************************************************************************************/
    /************************************************观察缓冲区***********************************************/
    /*******************************************************************************************************/
    // 创建观察缓冲区ObservationBuffer(),push_back()为在observation_buffers_这个vector中的最后添加一个元素
    observation_buffers_.push_back(
        boost::shared_ptr < ObservationBuffer > (new ObservationBuffer(topic, observation_keep_time, expected_update_rate, min_obstacle_height,
                                   max_obstacle_height, obstacle_range, raytrace_range, *tf_, global_frame_, sensor_frame, transform_tolerance)));

    // 检查是否将此缓冲区添加到标记观察缓冲区，此时marking_buffers_虽添加了观察区的数据，但还未调用传感器数据回调函数，其值为空值
    if (marking)
      marking_buffers_.push_back(observation_buffers_.back());

    // 检查是否还要将此缓冲区添加到清除观察缓冲区
    if (clearing)
      clearing_buffers_.push_back(observation_buffers_.back());

    ROS_DEBUG(
        "Created an observation buffer for source %s, topic %s, global frame: %s, "
        "expected update rate: %.2f, observation persistence: %.2f",
        source.c_str(), topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);

    /*******************************************************************************************************/
    /************************************************传感器数据***********************************************/
    /*******************************************************************************************************/
    // 分别针对不同的sensor类型（LaserScan、PointCloud、PointCloud2）注册不同的回调函数，相应的订阅者和filter还要插入相应的容器
    if (data_type == "LaserScan"){
      // 创建一个message_filters::Subscriber<sensor_msgs::LaserScan>类型的智能指针sub，用于订阅主题topic上的激光雷达扫描数据，并设置缓冲区大小为50
      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::LaserScan> > sub(new message_filters::Subscriber<sensor_msgs::LaserScan>(g_nh, topic, 50));
      // 创建一个tf2_ros::MessageFilter<sensor_msgs::LaserScan>类型的智能指针filter，将刚刚创建的sub作为输入源，并与全局坐标系转换器tf_关联，
      // 指定全局坐标系名为global_frame_，同样设置缓冲区大小为50，并且传递了节点句柄g_nh。
      boost::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::LaserScan> > filter(new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*sub, *tf_, global_frame_, 50, g_nh));

      if (inf_is_valid){
        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanValidInfCallback, this, _1, observation_buffers_.back()));
      }
      else{
        filter->registerCallback(boost::bind(&ObstacleLayer::laserScanCallback, this, _1, observation_buffers_.back()));
      }

      // 将刚创建的订阅者sub添加到observation_subscribers_容器中保存，以便后续管理或取消订阅
      observation_subscribers_.push_back(sub);
      // 将刚创建的消息过滤器filter添加到observation_notifiers_容器中保存，并通过调用其setTolerance方法设置容忍时间窗口为0.05秒，
      // 这意味着过滤器允许消息在这个时间内延迟到达并仍能进行正确的时间同步和坐标系变换。
      observation_notifiers_.push_back(filter);
      observation_notifiers_.back()->setTolerance(ros::Duration(0.05));
    }
    else if (data_type == "PointCloud"){
      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud>
          > sub(new message_filters::Subscriber<sensor_msgs::PointCloud>(g_nh, topic, 50));

      if (inf_is_valid){
       ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
      }

        boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud>
        > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud>(*sub, *tf_, global_frame_, 50, g_nh));
        filter->registerCallback(
          boost::bind(&ObstacleLayer::pointCloudCallback, this, _1, observation_buffers_.back()));

      observation_subscribers_.push_back(sub);
      observation_notifiers_.push_back(filter);
    }
    else{
      boost::shared_ptr < message_filters::Subscriber<sensor_msgs::PointCloud2> > sub(new message_filters::Subscriber<sensor_msgs::PointCloud2>(g_nh, topic, 50));

      if (inf_is_valid){
        ROS_WARN("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
      }

      boost::shared_ptr < tf2_ros::MessageFilter<sensor_msgs::PointCloud2>
      > filter(new tf2_ros::MessageFilter<sensor_msgs::PointCloud2>(*sub, *tf_, global_frame_, 50, g_nh));
      filter->registerCallback(
          boost::bind(&ObstacleLayer::pointCloud2Callback, this, _1, observation_buffers_.back()));

      observation_subscribers_.push_back(sub);
      observation_notifiers_.push_back(filter);
    }

    if (sensor_frame != ""){
      std::vector < std::string > target_frames;
      target_frames.push_back(global_frame_);
      target_frames.push_back(sensor_frame);
      observation_notifiers_.back()->setTargetFrames(target_frames);
    }
  }

  dsrv_ = NULL;
  setupDynamicReconfigure(nh);
}

void ObstacleLayer::setupDynamicReconfigure(ros::NodeHandle& nh){
  dsrv_ = new dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>(nh);
  dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig>::CallbackType cb = boost::bind(
      &ObstacleLayer::reconfigureCB, this, _1, _2);
  dsrv_->setCallback(cb);
}

ObstacleLayer::~ObstacleLayer(){
    if (dsrv_)
        delete dsrv_;
}
void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level){
  enabled_ = config.enabled;
  footprint_clearing_enabled_ = config.footprint_clearing_enabled;
  max_obstacle_height_ = config.max_obstacle_height;
  combination_method_ = config.combination_method;
}

void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer){
  // 将激光转换成点云
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message->header;

  // 激光数据装换成点云
  try{
    // 将激光雷达扫描数据转换到指定坐标系下的点云数据，这里通过调用transformLaserScanToPointCloud方法完成，并使用了TF库进行坐标变换
    projector_.transformLaserScanToPointCloud(message->header.frame_id, *message, cloud, *tf_);
  }
  catch (tf2::TransformException &ex){
    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s", global_frame_.c_str(),
             ex.what());
    projector_.projectLaser(*message, cloud);
  }
  catch (std::runtime_error &ex){
    ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
    return; //ignore this message
  }

  // 缓冲点云信息
  buffer->lock();
  buffer->bufferCloud(cloud);//实际上是sensor_msgs::PointCloud2 >>>pcl::PCLPointCloud2 >>> pcl::PointCloud < pcl::PointXYZ > ;   然后调用void ObservationBuffer::bufferCloud(const pcl::PointCloud<pcl::PointXYZ>& cloud);
  buffer->unlock();
}

void ObstacleLayer::laserScanValidInfCallback(const sensor_msgs::LaserScanConstPtr& raw_message, const boost::shared_ptr<ObservationBuffer>& buffer){
  // 把正的无限值转换成max_range
  float epsilon = 0.0001;  // 0.1毫米
  sensor_msgs::LaserScan message = *raw_message;
  for (size_t i = 0; i < message.ranges.size(); i++){
    float range = message.ranges[ i ];
    if (!std::isfinite(range) && range > 0){
      message.ranges[ i ] = message.range_max - epsilon;
    }
  }

  // project the laser into a point cloud
  sensor_msgs::PointCloud2 cloud;
  cloud.header = message.header;

  // project the scan into a point cloud
  try{
    // projector_是LaserProjection的实例，laserProjection也是ros中的库，不在navigation代码中
    projector_.transformLaserScanToPointCloud(message.header.frame_id, message, cloud, *tf_);
  }
  catch (tf2::TransformException &ex){
    ROS_WARN("High fidelity enabled, but TF returned a transform exception to frame %s: %s",
             global_frame_.c_str(), ex.what());
    projector_.projectLaser(message, cloud);
  }
  catch (std::runtime_error &ex){
    ROS_WARN("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s", ex.what());
    return; //ignore this message
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud);
  buffer->unlock();
}

// 处理缓冲点云pointcloud消息的回调函数
void ObstacleLayer::pointCloudCallback(const sensor_msgs::PointCloudConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer){
  sensor_msgs::PointCloud2 cloud2;
  if (!sensor_msgs::convertPointCloudToPointCloud2(*message, cloud2)){
    ROS_ERROR("Failed to convert a PointCloud to a PointCloud2, dropping message");
    return;
  }

  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(cloud2);
  buffer->unlock();
}

// 处理缓冲点云pointcloud2消息的回调函数
void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr& message, const boost::shared_ptr<ObservationBuffer>& buffer){
  // buffer the point cloud
  buffer->lock();
  buffer->bufferCloud(*message);
  buffer->unlock();
}

// 传入参数 robot_x、robot_y和robot_yaw 机器人位姿
// min_x、min_y、max_x和 max_y用于 更新边界轮廓
// 这个函数主要完成 clearing, marking以及确定bound。和静态地图类似，同样也是先判断是否是rolling地图，若是则更新地图原点
void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y){
  if (rolling_window_)// 判断是否调用滚动窗口，若是，则更新原点坐标
    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
  
  useExtraBounds(min_x, min_y, max_x, max_y);                               // 确定更新边界轮廓尺寸

  bool current = true;
  std::vector<Observation> observations, clearing_observations;
  current = current && getMarkingObservations(observations);                // 获取标记观测
  current = current && getClearingObservations(clearing_observations);      // 获取清除观测
  current_ = current;                                                       // 更新全局当前状态
  
  /**
   * 清除代价地图的方式也很好理解，以单线激光雷达的激光线为例子，当激光线发射出去之后，在某处检测到一个障碍物，
   * 那说明：从发射的地方至某处之间是free的，那么这之间的旧的障碍物应当被删除，这之间网格的代价值应当被修改为free
   * 
   * 有时会发现有些障碍并没有及时的被清除掉的原因：
   * 可能是Bresenham2D的round off error(舍入误差)造成的，它将特定单元格标记为障碍物，但下次当传感器读数瞬时发生变化时，
   * 它不再通过相同的路径跟踪它，因此，障碍物似乎保留在代价地图中。
   * 在 Bresenham2D 算法中添加了 2 行代码。这基本上清除了网格左侧和右侧的单元格，算法构造的线段通过该单元格。
   * 这会导致地图失去一些分辨率，但该解决方案在现实生活中运行良好。
   *
      Bresenham2D 代码修改：
      inline void bresenham2D( ActionType at, unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b, unsigned int offset, unsigned int max_length){
        unsigned int end = std::min(max_length, abs_da);
        for (unsigned int i = 0; i < end; ++i){
          at(offset);
          at(offset+1); // **ADDED THIS LINE**
          at(offset-1); // **ADDED THIS LINE**
          offset += offset_a;
          error_b += abs_db;
          if ((unsigned int)error_b >= abs_da){
            offset += offset_b;
            error_b -= abs_da;
          }
        }
        at(offset);
      }

      https://charon-cheung.github.io/2021/11/04/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/%E9%9A%9C%E7%A2%8D%E5%B1%825%20%20%E4%BB%8E%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE%E6%B8%85%E9%99%A4%E9%9A%9C%E7%A2%8D/

      障碍层6 加速清除代价地图中的障碍
      https://charon-cheung.github.io/2021/11/13/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/%E9%9A%9C%E7%A2%8D%E5%B1%826%20%20%E5%8A%A0%E9%80%9F%E6%B8%85%E9%99%A4%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE%E4%B8%AD%E7%9A%84%E9%9A%9C%E7%A2%8D/#%E9%9B%B7%E8%BE%BE%E6%97%A0%E6%B3%95%E5%8F%8A%E6%97%B6%E6%B8%85%E9%99%A4%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE%E4%B8%AD%E7%9A%84%E9%9A%9C%E7%A2%8D

      障碍层7 添加障碍
      https://charon-cheung.github.io/2021/11/15/%E8%B7%AF%E5%BE%84%E8%A7%84%E5%88%92/%E4%BB%A3%E4%BB%B7%E5%9C%B0%E5%9B%BE/%E9%9A%9C%E7%A2%8D%E5%B1%827%20%20%E6%B7%BB%E5%8A%A0%E9%9A%9C%E7%A2%8D/
      
   *  
   */

  for (unsigned int i = 0; i < clearing_observations.size(); ++i){            // 光线跟踪自由空间
    raytraceFreespace(clearing_observations[i], min_x, min_y, max_x, max_y);  // 清理出传感器 到 被测物之间的区域，标记为FREE_SPACE ，这样就清除完毕了
  }
  // 根据测量数据完成clear 操作之后，就开始了mark 操作，对每个测量到的点，标记为obstacle，将新障碍物放入优先级队列...每个都以零优先级开始
  for (std::vector<Observation>::const_iterator it = observations.begin(); it != observations.end(); ++it){
    const Observation& obs = *it;
    const sensor_msgs::PointCloud2& cloud = *(obs.cloud_);
    double sq_obstacle_range = obs.obstacle_range_ * obs.obstacle_range_;

    sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
    sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");
    sensor_msgs::PointCloud2ConstIterator<float> iter_z(cloud, "z");

    for (; iter_x !=iter_x.end(); ++iter_x, ++iter_y, ++iter_z){
      double px = *iter_x, py = *iter_y, pz = *iter_z;

      // 如果障碍物离机器人太高，将不被考虑在内
      if (pz > max_obstacle_height_){
        ROS_DEBUG("The point is too high");
        continue;
      }

      // 计算从命中点到点云原点的平方距离
      double sq_dist = (px - obs.origin_.x) * (px - obs.origin_.x) + (py - obs.origin_.y) * (py - obs.origin_.y) + (pz - obs.origin_.z) * (pz - obs.origin_.z);

      // 如果该点足够远，将不会考虑
      if (sq_dist >= sq_obstacle_range){
        ROS_DEBUG("The point is too far away");
        continue;
      }

      // 需要计算观测的地图坐标
      unsigned int mx, my;
      if (!worldToMap(px, py, mx, my)){
        ROS_DEBUG("Computing map coords failed");
        continue;
      }

      unsigned int index = getIndex(mx, my);
      costmap_[index] = LETHAL_OBSTACLE;
      // 更新参数中指定的边界框以包括位置(x,y)
      touch(px, py, min_x, min_y, max_x, max_y);
    }
  }

  updateFootprint(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y);
}
// 更新机器人的footprint
void ObstacleLayer::updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y, double* max_x, double* max_y){
    if (!footprint_clearing_enabled_) return;
    // 这里获得了在当前机器人位姿（robot_x, robot_y, robot_yaw）条件下，机器人轮廓点在global坐标系下的值
    transformFootprint(robot_x, robot_y, robot_yaw, getFootprint(), transformed_footprint_);

    for (unsigned int i = 0; i < transformed_footprint_.size(); i++){
      touch(transformed_footprint_[i].x, transformed_footprint_[i].y, min_x, min_y, max_x, max_y);// 再次保留或者扩张Bounds
    }
}

/**
 * 更新障碍地图代价，将机器人足迹范围内设置为 FREE_SPACE，并且在 bound 范围内将本层障碍地图的内容合并到主地图上。
 * 障碍物层是将传感器检测到的点云投影到地图中，不同的传感器来源会分别存到Observation类中。只要遍历存储Observation的容器，
 * 将检测到的点云的每个点分别投影到地图中，将对应的网格的代价值设为LETHAL_OBSTACLE，实现函数在ObstacleLayer::updateBounds中
 */
void ObstacleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j){// 更新代价值
  if (footprint_clearing_enabled_){
    setConvexPolygonCost(transformed_footprint_, costmap_2d::FREE_SPACE); // 设置机器人轮廓所在区域为FREE_SPACE
  }
  // 在 bound 范围内将本层障碍地图的内容合并到 master_grid ，combination_method_ 更新 master_grid 的方法
  switch (combination_method_){
    case 0:  // Overwrite调用的CostmapLayer提供的方法
      // 覆盖更新，地图层的每个有效值（不含未知状态（NO_INFORMATION））写进master_grid
      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j);
      break;
    case 1:  // Maximum
      // 最大值更新，主栅格的新值取master_grid中各层中的最大值
      // 特殊情况:
    	// a) 如果主代价值是未知状态（NO_INFORMATION），则被覆盖更新
    	// b) 如果所操作的地图层的代价值是未知状态（NO_INFORMATION），则主代价值不变
      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
      break;
    default:  // Nothing
      break;
  }
}

// 添加静态观察
void ObstacleLayer::addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing)
{
  if (marking)
    static_marking_observations_.push_back(obs);
  if (clearing)
    static_clearing_observations_.push_back(obs);
}
// 清除静态观察
void ObstacleLayer::clearStaticObservations(bool marking, bool clearing)
{
  if (marking)
    static_marking_observations_.clear();
  if (clearing)
    static_clearing_observations_.clear();
}
// 获取标记观察
bool ObstacleLayer::getMarkingObservations(std::vector<Observation>& marking_observations) const
{
  bool current = true;
  // get the marking observations
  for (unsigned int i = 0; i < marking_buffers_.size(); ++i)
  {
    marking_buffers_[i]->lock();
    marking_buffers_[i]->getObservations(marking_observations);
    current = marking_buffers_[i]->isCurrent() && current;
    marking_buffers_[i]->unlock();
  }
  marking_observations.insert(marking_observations.end(),
                              static_marking_observations_.begin(), static_marking_observations_.end());
  return current;
}
// 获取清除观察，与标记观察是相对的
bool ObstacleLayer::getClearingObservations(std::vector<Observation>& clearing_observations) const
{
  bool current = true;
  // 获取用于清除的观察
  for (unsigned int i = 0; i < clearing_buffers_.size(); ++i)
  {
    clearing_buffers_[i]->lock();
    clearing_buffers_[i]->getObservations(clearing_observations);
    current = clearing_buffers_[i]->isCurrent() && current;
    clearing_buffers_[i]->unlock();
  }
  clearing_observations.insert(clearing_observations.end(),
                              static_clearing_observations_.begin(), static_clearing_observations_.end());
  return current;
}

void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, double* min_x, double* min_y, double* max_x, double* max_y){
  double ox = clearing_observation.origin_.x;
  double oy = clearing_observation.origin_.y;
  const sensor_msgs::PointCloud2 &cloud = *(clearing_observation.cloud_);

  // 获取传感器原点的地图坐标
  unsigned int x0, y0;
  if (!worldToMap(ox, oy, x0, y0)){
    ROS_WARN_THROTTLE(1.0, "The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.", ox, oy);
    return;
  }

  // 内循环外预先计算地图的终点，之后需要
  double origin_x = origin_x_, origin_y = origin_y_;
  double map_end_x = origin_x + size_x_ * resolution_;
  double map_end_y = origin_y + size_y_ * resolution_;

  touch(ox, oy, min_x, min_y, max_x, max_y);

  // 点云中的每个点与原点建一条线，如果有障碍物在这条线上，则被清除. 障碍物层是二维的，所以没有z方向
  sensor_msgs::PointCloud2ConstIterator<float> iter_x(cloud, "x");
  sensor_msgs::PointCloud2ConstIterator<float> iter_y(cloud, "y");

  for (; iter_x != iter_x.end(); ++iter_x, ++iter_y){
    double wx = *iter_x;
    double wy = *iter_y;

    // 确保光线跟踪到的点没有在代价地图外，如果有必要，做尺度缩放
    double a = wx - ox;
    double b = wy - oy;

    // 光线追踪最小值是原点
    if (wx < origin_x){
      double t = (origin_x - ox) / a;
      wx = origin_x;
      wy = oy + b * t;
    }
    if (wy < origin_y){
      double t = (origin_y - oy) / b;
      wx = ox + a * t;
      wy = origin_y;
    }

    // 光线追踪的最大值是地图终点
    if (wx > map_end_x){
      double t = (map_end_x - ox) / a;
      wx = map_end_x - .001;
      wy = oy + b * t;
    }
    if (wy > map_end_y){
      double t = (map_end_y - oy) / b;
      wx = ox + a * t;
      wy = map_end_y - .001;
    }

    // 成适当尺寸后，获得终点在map下的坐标
    unsigned int x1, y1;

    // 检查wx, wy在map中是否有效
    if (!worldToMap(wx, wy, x1, y1))
      continue;

    // 求出传感器数据原点和传感器原点连城的线段和边界的交点，然后清除传感器原点到交点之间的障碍物
    unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
    MarkCell marker(costmap_, FREE_SPACE);
    raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range); // 沿着光线清除障碍物，会将所有在(x0,y0)->(x1,y1)之间的所有cell标记为FREE_SPACE
    updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y); // 会根据测量的距离，更新扩张（min_x, min_y, max_x, max_y）
  }
}

void ObstacleLayer::activate()
{
  // if we're stopped we need to re-subscribe to topics
  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
  {
    if (observation_subscribers_[i] != NULL)
      observation_subscribers_[i]->subscribe();
  }

  for (unsigned int i = 0; i < observation_buffers_.size(); ++i)
  {
    if (observation_buffers_[i])
      observation_buffers_[i]->resetLastUpdated();
  }
}
void ObstacleLayer::deactivate()
{
  for (unsigned int i = 0; i < observation_subscribers_.size(); ++i)
  {
    if (observation_subscribers_[i] != NULL)
      observation_subscribers_[i]->unsubscribe();
  }
}

void ObstacleLayer::updateRaytraceBounds(double ox, double oy, double wx, double wy, double range, double* min_x, double* min_y, double* max_x, double* max_y)
{
  double dx = wx-ox, dy = wy-oy;
  double full_distance = hypot(dx, dy);
  double scale = std::min(1.0, range / full_distance);
  double ex = ox + dx * scale, ey = oy + dy * scale;
  touch(ex, ey, min_x, min_y, max_x, max_y);
}

void ObstacleLayer::reset()
{
    deactivate();
    resetMaps();
    current_ = true;
    activate();
}

}  // namespace costmap_2d
